import cv2
import numpy as np
import math
# import serial
# from skimage import data, exposure, img_as_float

def Routing(point1, point2, remap_point, image):
    p1 = remap_point[point1[0]][point1[1]]
    p2 = remap_point[point2[0]][point2[1]]
    # cv2.circle(im_crop, p1, 2, (0, 0, 255), 2)  # 画圆心
    # cv2.circle(im_crop, p2, 2, (0, 0, 255), 2)  # 画圆心
    # 如果x轴相等，那就是y轴需要移动
    if (p1[0] == p2[0]):
        if p2[1] > p1[1]:
            a = image[p1[1]:p2[1], p1[0]] > 0
        else:
            a = image[p2[1]:p1[1], p1[0]] > 0
        sum = np.sum(a)
        if sum > 0:
            return False
    if (p1[1] == p2[1]):
        if (p1[0] > p2[0]):
            a = image[p1[1], p2[0]:p1[0]] > 0
        else:
            a = image[p1[1], p1[0]:p2[0]] > 0
        sum = np.sum(a)
        if sum > 0:
            return False
    return True


def get_route(point0, point1, im_crop, edge, points):
    gray_image = cv2.cvtColor(im_crop, cv2.COLOR_RGB2GRAY)

    skip = []
    bianli_list = []
    route = []
    head = 0  # 队列头
    # rear = 0#队列尾巴
    track = []
    bianli_list.append([point0[0], point0[1], -1, 1])
    while bianli_list[head][0] != point1[0] or bianli_list[head][1] != point1[1]:
        x = bianli_list[head][0]
        y = bianli_list[head][1]
        r = bianli_list[head][3]
        point_lf = [x - 1, y]
        point_rg = [x + 1, y]
        point_up = [x, y - 1]
        point_dn = [x, y + 1]
        if 0 <= point_lf[0] < 10 and 0 <= point_lf[1] < 10:
            if point_lf[0] != bianli_list[bianli_list[head][2]][0] or point_lf[1] != bianli_list[bianli_list[head][2]][
                1]:
                if Routing([x, y], point_lf, points, edge) == True:
                    bianli_list.append([point_lf[0], point_lf[1], head, r + 1])

        if 0 <= point_rg[0] < 10 and 0 <= point_rg[1] < 10:
            if point_rg[0] != bianli_list[bianli_list[head][2]][0] or point_rg[1] != bianli_list[bianli_list[head][2]][
                1]:
                if Routing([x, y], point_rg, points, edge) == True:
                    bianli_list.append([point_rg[0], point_rg[1], head, r + 1])

        if 0 <= point_up[0] < 10 and 0 <= point_up[1] < 10:
            if point_up[0] != bianli_list[bianli_list[head][2]][0] or point_up[1] != bianli_list[bianli_list[head][2]][
                1]:
                if Routing([x, y], point_up, points, edge) == True:
                    bianli_list.append([point_up[0], point_up[1], head, r + 1])

        if 0 <= point_dn[0] < 10 and 0 <= point_dn[1] < 10:
            if point_dn[0] != bianli_list[bianli_list[head][2]][0] or point_dn[1] != bianli_list[bianli_list[head][2]][
                1]:
                if Routing([x, y], point_dn, points, edge) == True:
                    bianli_list.append([point_dn[0], point_dn[1], head, r + 1])
        head += 1

    while (bianli_list[head][2] != -1):
        x0 = bianli_list[head][0]
        y0 = bianli_list[head][1]
        route.append([x0, y0])
        pre_index = bianli_list[head][2]
        x1 = bianli_list[pre_index][0]
        y1 = bianli_list[pre_index][1]
        cv2.line(im_crop, (points[x0][y0][0], points[x0][y0][1]), (points[x1][y1][0], points[x1][y1][1]), (0, 0, 255),2)
        cv2.imshow('1', im_crop)
        # cv2.waitKey(0)
        x, y = (points[x0][y0][0], points[x0][y0][1])

        xx,yy =(points[x1][y1][0], points[x1][y1][1])
        track.append([x,y])

        head = pre_index

    track.append([xx, yy])
    return route,skip,track
    # cv2.imshow("line",im_crop)
    # cv2.waitKey()


def get_short_route(point0, baozang_points, im_crop, edge, points):
    gray_image = cv2.cvtColor(im_crop, cv2.COLOR_RGB2GRAY)
    # ret, mask = cv2.threshold(gray_image, 50, 255, cv2.THRESH_BINARY_INV)

    ret, mask = cv2.threshold(gray_image, 0, 255, cv2.THRESH_BINARY  + cv2.THRESH_OTSU)
    # print(ret)
    mask = 255- mask

    bianli_list = []
    head = 0  # 队列头
    route = []
    total_route = []
    skips = []
    skip = []
    tracks = []
    track =[]
    # rear = 0#队列尾巴
    bianli_list.append([point0[0], point0[1], -1, 1])
    while (baozang_points):
        while [bianli_list[head][0], bianli_list[head][1]] not in baozang_points:
            x = bianli_list[head][0]
            y = bianli_list[head][1]
            r = bianli_list[head][3]
            point_lf = [x - 1, y]
            point_rg = [x + 1, y]
            point_up = [x, y - 1]
            point_dn = [x, y + 1]
            if 0 <= point_lf[0] < 10 and 0 <= point_lf[1] < 10:
                if point_lf[0] != bianli_list[bianli_list[head][2]][0] or point_lf[1] != \
                        bianli_list[bianli_list[head][2]][1]:
                    if Routing([x, y], point_lf, points, edge) == True:
                        bianli_list.append([point_lf[0], point_lf[1], head, r + 1])

            if 0 <= point_rg[0] < 10 and 0 <= point_rg[1] < 10:
                if point_rg[0] != bianli_list[bianli_list[head][2]][0] or point_rg[1] != \
                        bianli_list[bianli_list[head][2]][1]:
                    if Routing([x, y], point_rg, points, edge) == True:
                        bianli_list.append([point_rg[0], point_rg[1], head, r + 1])

            if 0 <= point_up[0] < 10 and 0 <= point_up[1] < 10:
                if point_up[0] != bianli_list[bianli_list[head][2]][0] or point_up[1] != \
                        bianli_list[bianli_list[head][2]][1]:
                    if Routing([x, y], point_up, points, edge) == True:
                        bianli_list.append([point_up[0], point_up[1], head, r + 1])

            if 0 <= point_dn[0] < 10 and 0 <= point_dn[1] < 10:
                if point_dn[0] != bianli_list[bianli_list[head][2]][0] or point_dn[1] != \
                        bianli_list[bianli_list[head][2]][1]:
                    if Routing([x, y], point_dn, points, edge) == True:
                        bianli_list.append([point_dn[0], point_dn[1], head, r + 1])
            head += 1
        new_start_point = [bianli_list[head][0], bianli_list[head][1]]
        baozang_points.remove(new_start_point)
        while (bianli_list[head][2] != -1):
            x0 = bianli_list[head][0]
            y0 = bianli_list[head][1]
            route.append([x0, y0])
            pre_index = bianli_list[head][2]
            x1 = bianli_list[pre_index][0]
            y1 = bianli_list[pre_index][1]
            cv2.line(im_crop, (points[x0][y0][0], points[x0][y0][1]), (points[x1][y1][0], points[x1][y1][1]),
                     (51, 255, 255), 2)  # BGR黄色
            cv2.imshow('1',im_crop)
            # cv2.waitKey(0)
            x, y = (points[x0][y0][0], points[x0][y0][1])

            xx,yy = (points[x1][y1][0], points[x1][y1][1])
            track.append([x,y])

            head = pre_index

        track.append([xx, yy])

        route.append([bianli_list[head][0], bianli_list[head][1]])
        total_route.append(route[:])
        route.clear()

        # skip.append(0)
        skips.append(skip[::-1])
        skip = []

        tracks.append(track[::-1])
        track = []
        bianli_list.clear()
        bianli_list.append([new_start_point[0], new_start_point[1], -1, 1])
    route,skip,track = get_route([9, 0], new_start_point, im_crop, edge, points)
    skips.append(skip)
    total_route.append(route)
    tracks.append(track)
    cv2.imshow('mk',mask)
    return im_crop, total_route,skips,tracks


def find_rect(img):
    edge = cv2.Canny(img, 200, 250);
    # binaly_img   = cv2.dilate(edge, kernel, iterations = 1)
    contours = cv2.findContours(edge, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)

    # 3print("-----------------------------")
    # print((contours[2]))
    bounding_boxes = [cv2.boundingRect(cnt) for cnt in contours[-2]]
    index_list = []
    # print(contours[2][0])
    # 找到有内轮廓的轮廓
    for i in range(len(bounding_boxes)):
        hie = contours[1][0][i]
        j = 0;
        while (hie[2] > 0):
            j += 1
            hie = contours[1][0][hie[2]]
        if j > 2:
            index_list.append(i)

    for i, box in enumerate(bounding_boxes):
        if i in index_list:
            if box[2] > 1.5 * box[3] or box[2] < 0.6 * box[3] or box[3] * box[2] > 10000:
                index_list.remove(i)

            # else:
            #  cv2.rectangle(img, (box[0], box[1]), (box[0] + box[2], box[1] + box[3]), (0, 255, 0), 2)

    nei_list = []
    for i in index_list:
        hie = contours[1][0][i]
        j = 0;
        while (hie[2] > 0):
            hie = contours[1][0][hie[2]]
            h1 = bounding_boxes[i][3]
        nei_list.append(hie[3])

    nei_list.sort()

    index_list = []
    for l in nei_list:
        if l not in index_list:
            index_list.append(l)
            box = bounding_boxes[l]
            cv2.rectangle(img, (box[0], box[1]), (box[0] + box[2], box[1] + box[3]), (0, 255, 0), 2)

    points = []
    cv2.imshow("img", img)
    # cv2.waitKey(10)
    if len(index_list) >= 4:
        for l in index_list:
            x, y, w, h = bounding_boxes[l]
            if w * h < 400:
                points.append([int(x + w / 2), int(y + h / 2)])

    if (len(points) == 4):
        points.sort()
        if (points[0][1] < points[1][1]):
            left_top_p = points[0]
            left_down_p = points[1]
        else:
            left_top_p = points[1]
            left_down_p = points[0]

        if (points[2][1] < points[3][1]):
            right_top_p = points[2]
            right_down_p = points[3]
        else:
            right_top_p = points[3]
            right_down_p = points[2]

        return left_top_p, left_down_p, right_top_p, right_down_p, h1
    return 0, 0, 0, 0, 0


def img_jiaozheng(left_top_p, left_down_p, right_top_p, right_down_p, frame, h1):
    # if (right_down_p[0]-left_top_p[0])>(right_down_p[1]-left_top_p[1]):
    #  w = right_down_p[0]-left_top_p[0]
    # else:
    # w = right_down_p[1]-left_top_p[1]
    tl = left_top_p
    tr = right_top_p
    br = right_down_p
    bl = left_down_p

    widthA = np.sqrt(((br[0] - bl[0]) ** 2) + ((br[1] - bl[1]) ** 2))
    widthB = np.sqrt(((tr[0] - tl[0]) ** 2) + ((tr[1] - tl[1]) ** 2))
    maxWidth = max(int(widthA), int(widthB))

    heightA = np.sqrt(((tr[0] - br[0]) ** 2) + ((tr[1] - br[1]) ** 2))
    heightB = np.sqrt(((tl[0] - bl[0]) ** 2) + ((tl[1] - bl[1]) ** 2))
    maxheight = max(int(heightA), int(heightB))

    target_lf_t = [0, 0]
    target_rg_t = [maxWidth - 1, 0]
    target_rg_d = [maxWidth - 1, maxheight - 1]
    target_lf_d = [0, maxheight - 1]

    # 开始校正
    pointSrc = np.float32([tl, tr, br, bl])  # 原始图像中 4点坐标
    pointDst = np.float32([target_lf_t, target_rg_t, target_rg_d, target_lf_d])  # 变换图像中 4点坐标
    MP = cv2.getPerspectiveTransform(pointSrc, pointDst)  # 计算投影变换矩阵 M
    imgP = cv2.warpPerspective(frame, MP, (maxWidth, maxheight))
    im_crop = imgP[int(maxheight / 22):maxheight - int(maxheight / 22),
              int(maxWidth / 22):maxWidth - int(maxWidth / 22), :]

    cv2.imshow("img", im_crop)  # 矫正后的图片
    cv2.waitKey(10)

    return im_crop


def get_points(im_crop):
    gray_image = cv2.cvtColor(im_crop, cv2.COLOR_RGB2GRAY)
    # ret, edge = cv2.threshold(gray_image, 50, 255, cv2.THRESH_BINARY_INV)

    ret, edge = cv2.threshold(gray_image, 0, 255, cv2.THRESH_BINARY  + cv2.THRESH_OTSU)
    print(ret)
    edge = 255- edge
    # edge =cv2.Canny(gray_image,200,250)+edge;
    kernel = np.ones((3, 3), np.uint8)
    baozang_points = []

    # bz = []
    points = []
    edge = cv2.dilate(edge, kernel, iterations=1)

    kernel = np.ones((1, int(im_crop.shape[1] / 30)), np.uint8)
    edge1 = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kernel, iterations=2);
    lines = cv2.HoughLines(edge1, 1, np.pi / 2, 80, 200)
    mid_p_x = []
    if lines is not None:
        lines1 = lines[:, 0, :]
        # mid_p_x = []
        for rho, theta in lines1[:]:
            a = np.cos(theta)
            b = np.sin(theta)
            y0 = b * rho
            if y0 > 0:
                mid_p_x.append(y0)

    kernel = np.ones((int(im_crop.shape[0] / 30), 1), np.uint8)
    binaly_img2 = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kernel, iterations=2);
    lines = cv2.HoughLines(binaly_img2, 1, np.pi / 2, 50, 150)
    # lines1 = lines[:, 0, :]
    mid_p_y = []
    if lines is not None:

        lines1 = lines[:, 0, :]
        for rho, theta in lines1[:]:
            a = np.cos(theta)
            b = np.sin(theta)
            x0 = a * rho
            if x0 > 0:
                mid_p_y.append(x0)

    if (len(mid_p_y) > 5 and len(mid_p_x) > 5):
        mid_p_y.sort()
        mid_p_x.sort()
        len_x = len(mid_p_x)
        len_y = len(mid_p_y)
        gap_x = []
        gap_y = []
        for i in range(len_x - 1):
            gap_x.append(mid_p_x[i + 1] - mid_p_x[i])
        for i in range(len_y - 1):
            gap_y.append(mid_p_y[i + 1] - mid_p_y[i])
        width, height, c = im_crop.shape
        gap_av_x = width / 12  # np.mean(gap_x)
        gap_av_y = height / 12  # np.mean(gap_y)
        remove_x = []
        remove_y = []
        for i, g in enumerate(gap_x):
            if g < gap_av_x * 0.7:
                remove_x.append(mid_p_x[i + 1])
            if g > gap_av_x * 1.5:
                mid_p_x.append(mid_p_x[i] + int((mid_p_x[i + 1] - mid_p_x[i]) / 2))
        for i, g in enumerate(gap_y):
            if g < gap_av_y * 0.7:
                remove_y.append(mid_p_y[i + 1])
            if g > gap_av_y * 1.5:
                mid_p_y.append(mid_p_y[i] + int((mid_p_y[i + 1] - mid_p_y[i]) / 2))
        for x in remove_x:
            mid_p_x.remove(x)
        for y in remove_y:
            mid_p_y.remove(y)
        im = im_crop.copy()
        for x in mid_p_y:
            cv2.line(im, (int(x), 0), (int(x), 600), (0, 0, 255), 2)
        for x in mid_p_x:
            cv2.line(im, (0, int(x)), (600, int(x)), (0, 0, 255), 2)

        if len(mid_p_x) == 11 and len(mid_p_y) == 11:
            points = []
            mid_p_y.sort();
            mid_p_x.sort();
            for i in range(10):
                x_list = []
                y = mid_p_y[i]
                for j in range(10):
                    x = mid_p_x[j]
                    x_list.append([int(y + gap_av_y / 2), int(x + gap_av_x / 2)])
                    # cv2.circle(im_crop, (int(y+gap_av_y/2),int(x+gap_av_x/2)), 3, [0, 0, 255])

                points.append(x_list)
            kenel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (int(gap_av_y / 3), int(gap_av_x / 3)))
            yuan = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kenel, iterations=1);
            # circles = cv2.HoughCircles(edge, cv2.HOUGH_GRADIENT, 1, 30, param1=100, param2=200, minRadius=0, maxRadius=0)
            contours = cv2.findContours(yuan, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
            bounding_boxes = [cv2.boundingRect(cnt) for cnt in contours[-2]]

            for bbox in bounding_boxes:
                [x, y, w, h] = bbox

                # baozang_point.append([int(x+w/2),int(y+h/2)])
                if (x > mid_p_y[0]) and ((x + w / 2) < mid_p_y[10]):
                    for i in range(10):
                        if (x - points[i][0][0]) < gap_av_y / 2:
                            break

                    for j in range(10):
                        if (y - points[0][j][1]) < gap_av_x / 2:
                            baozang_points.append([i, j]);
                            break

                    for p in baozang_points:
                        x = points[p[0]][p[1]]
                        cv2.circle(im, (x[0], x[1]), 5, [255, 0, 0])
                        # bz.append((x[0], x[1]))
        else:
            kenel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (int(gap_av_y / 3), int(gap_av_x / 3)))
            yuan = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kenel, iterations=1);


    else:
        kenel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (int(30 / 3), int(30 / 3)))
        yuan = cv2.morphologyEx(edge, cv2.MORPH_OPEN, kenel, iterations=1);

        # return binaly_img2+edge1,points,baozang_point
        # cv2.imshow("yuan",yuan)
        # cv2.waitKey(10)
    kenel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))
    edge1 = binaly_img2 + edge1 + cv2.morphologyEx(edge - yuan, cv2.MORPH_OPEN, kenel, iterations=1);
    # edge1 = yuan
    # cv2.imshow('shange',im) # 迷宫格
    cv2.waitKey(10)
    # cv2.imshow("edge1",edge1) #边缘图像
    cv2.waitKey(10)
    # cv2.circle(im_crop, (int(y+gap_av_y/2),int(x+gap_av_x/2)), 5, [0, 0, 255])
    # edge2=binaly_img2+edge1
    return points, baozang_points, edge1

    # cv2.line(im_crop, (mid_p_y[0], 0), (mid_p_y[0], 600), (0, 0, 255), 2)

    # cv2.line(im_crop, (mid_p_y[10], 0), (mid_p_y[10], 600), (0, 0, 255), 2)


def del_img():
    kernel = np.ones((3, 3), np.uint8)
    cap = cv2.VideoCapture(0)  # 调用摄像头‘0’一般q是打开电脑自带摄像头，‘1’是打开外部摄像头（只有一个摄像头的情况）
    width = 800
    height = 600
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)  # 设置图像宽度
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)  # 设置图像高度
    # 显示图像
    while True:
        ret, frame = cap.read()  # 读取图像(frame就是读取的视频帧，对frame处理就是对整个视频的处理)
        # print(ret)#
        #######例如将图像灰度化处理，
        img = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)  # 转灰度图

        # img = exposure.adjust_gamma(img, 0.8)
        # img = cv2.equalizeHist(img)
        # cv2.imshow("imcrop",img)
        input = cv2.waitKey(10)
        # gray_image = cv2.medianBlur(img,3)
        left_top_p, left_down_p, right_top_p, right_down_p, h1 = find_rect(img)
        if (left_top_p != 0):
            im_crop = img_jiaozheng(left_top_p, left_down_p, right_top_p, right_down_p, frame, h1)
            # im_crop = im_crop[:,:,::-1]
            cv2.imshow("jiaozheng", im_crop)  # 矫正后的图片

            im = im_crop.copy()
            input = cv2.waitKey(10)
            points, baozang_point, edge = get_points(im_crop)
            # print(bz)
            if len(baozang_point) == 8:
                return edge, points, baozang_point, im_crop,im
        # return 0,0,0


def route_convert_instruct(route):
    instruct_t = []
    instruct = []
    instruct.append(3)
    pre_dir = [1, 0]
    for r in route:
        instruct = []
        length = len(r)
        for i in range(2, len(r)):
            x = [r[length - i - 1][0] - r[length - i][0], r[length - i - 1][1] - r[length - i][1]]
            if (x == pre_dir):
                instruct.append(3)
            elif (x[0] < 0 and pre_dir[1] > 0):
                instruct.append(0)
            elif (x[0] < 0 and pre_dir[1] < 0):
                instruct.append(1)
            elif (x[0] > 0 and pre_dir[1] > 0):
                instruct.append(1)
            elif (x[0] > 0 and pre_dir[1] < 0):
                instruct.append(0)
            elif (x[1] < 0 and pre_dir[0] < 0):
                instruct.append(0)
            elif (x[1] < 0 and pre_dir[0] > 0):
                instruct.append(1)
            elif (x[1] > 0 and pre_dir[0] > 0):
                instruct.append(0)
            elif (x[1] > 0 and pre_dir[0] < 0):
                instruct.append(1)
            pre_dir = x
        # instruct.append(3)
        instruct.append(2)
        pre_dir = [-pre_dir[0], -pre_dir[1]]
        instruct_t.append(instruct[:])
        instruct.clear()
    return instruct_t


def send_data(ser, ins):
    for d in ins:
        for i in d:
            if (i == 0):
                ser.write(b'0')  # turn right
            if (i == 1):
                ser.write(b'1')  # turn left
            if (i == 2):
                ser.write(b'2')  # turn back
            if (i == 3):
                ser.write(b'3')  # go farward

        ser.write(b'4')  # shibiebaozang

    ser.write(b'5')  # tingche


def encoder(tracks,im,mask):
    mask = cv2.cvtColor(mask, cv2.COLOR_RGB2GRAY)
    ret, mask = cv2.threshold(mask, 0, 255, cv2.THRESH_BINARY  + cv2.THRESH_OTSU)
    # print(ret)
    mask = 255- mask

    cv2.imwrite('1.png',mask)
    ans = []
    mask[:2,:] = 255
    mask[-2:,:] = 255
    h,w = mask.shape
    for track in tracks:
        num = len(track)
        an = []
        print('*'*20)
        for i in range(1,num-1):

            x0, y0 = track[i-1]
            xx,yy = track[i]
            x2,y2 = track[i+1]

            x,y = xx,yy
            vs = 0
            for i in range(30):
                x1 = x - 30 + i
                v = mask[y, x1]
                if v > 100:
                    vs += 1
                    break

            for i in range(30):
                x1 = x + i
                v = mask[y, x1]
                if v > 100:
                    vs += 1
                    break

            hs = 0
            for i in range(30):
                y1 = y - 30 + i
                v = mask[max(0,y1), x]
                if v > 100:
                    hs += 1
                    break

            for i in range(30):
                y1 = y + i
                v = mask[min(y1,h-1), x]
                if v > 100:
                    hs += 1
                    break
            # print(vs,hs)
            if  vs > 1 or hs > 1:
                sp = 1
            else:
                sp = 0

            x1, y1 = xx,yy

            m = -1
            if (abs(y1-y0)<5 and x1-x0>10 and abs(x2-x1)<5 and y1-y2>10) or (abs(x1-x0)<5 and y1-y0>10 and abs(y2-y1)<5 and x2-x1>10) or\
                        (abs(x1-x0)<5 and y0-y1>10 and abs(y1-y2)<5 and x1-x2>10) or (x0-x1>10 and abs(y1-y0)<5 and y2-y1>10 and abs(x2-x1)<5):
                m = 1
                # an.append(1)

            elif (abs(x1-x0)<5 and abs(x2-x1)<5) or (abs(y1-y0)<5 and abs(y2-y1)<5):
                if sp==0:
                    m =3
                    # an.append(3)
                else:
                    pass
            else:
                m = 0
            if m!=-1:
                an.append(m)

            print(vs,hs)
            print(x0,y0,x1,y1,x2,y2,sp,m)
            cv2.circle(im, (x1, y1), 5, [255, 0, 0])
            cv2.imshow('1', im)
            cv2.imshow('mk', mask)

            # cv2.waitKey(0)
        an = [str(cc) for cc in an]
        ans.append(an)


    return ans



def detect():
    # ser = serial.Serial("/dev/ttyAMA0",115200) #启动串口
    # ser.write(b'')
    edge, points, baozang_point, im_crop,im = del_img()
    res = []

    print(baozang_point)
    if (len(baozang_point) == 8):
        im_crop1, total_r,skips,tracks = get_short_route([0, 9], baozang_point, im_crop, edge, points)
        # skips = [[0] + c for c in skips]
        r = encoder(tracks,im_crop1,im)

        print(r)
        return r
        # print(len(skips[0]),len(tracks[0]))
        # for track in tracks:
        #     im = im_crop1.copy()
        #     for tr in track:
        #         x,y = tr
        #
        #
        #         cv2.circle(im, (x,y), 5, [255, 0, 0])
        #         cv2.imshow('1', im)
        #
        #         cv2.waitKey(0)

    #         print(tracks)
    #         ins = route_convert_instruct(total_r)
    #         # send_data(ser,ins)  #通过串口发送数据
    #         # print(ins,skips)
    #
    #         for vs, sps in zip(ins, skips):
    #             r = []
    #
    #             for v, sp in zip(vs, sps):
    #                 if not (sp == 1 and v==3):
    #
    #                     r.append(v)
    #
    #             res.append(r)
    # print(skips)
    # print(ins)
    # print(res)

    # while (True):
    #     cv2.imshow('lujing', im_crop1)  # 路径显示
    #     input = cv2.waitKey(10)
    #     if input == ord('q'):
    #         break
    # return res
    # cap.release()#释放摄像头


# cv2.destroyAllWindows()#销毁窗口

if __name__ == '__main__':
    detect()